#include "shoot.h"
#include "robot_def.h"
#include "gpio.h"
#include "dji_motor.h"
#include "message_center.h"
#include "bsp_dwt.h"
#include "general_def.h"
#include "robot_cmd.h"
#include "bsp_usart.h"
#include "usart.h"
#include "stdio.h"
#include "string.h"
#include "oled.h"

// static int outpost_speed = 28040;

// static int base_speed = 34200;

uint8_t shoot_speed_blt_send = 0;

uint8_t shoot_speed_blt_receive = 0;

static float shoot_speed =34000;
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *Friction_F_L, *Friction_F_R;
static DJIMotorInstance *Friction_B_L, *Friction_B_R;
static DJIMotorInstance *Loader;

static Publisher_t *shoot_pub;
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
static Subscriber_t *shoot_sub;
static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息

//无人知晓怎么添加的摩擦轮前馈
//static float shoot_ff = 0;

//串口实例，用于和其他开发板或激光雷达通信
static USARTInstance *blt_usart_instance;

//oled显示的工具拼接字符串
static char oled_buf[50] = { 0 };

//串口的回调函数
static void uart_callback(){}

// dwt定时,计算冷却用
static float hibernate_time = 0, dead_time = 0;
static float load_angle_set = 0;

//发射机构的初始化
void ShootInit()
{
    // shoot_speed.B_L = 20000; 
    // shoot_speed.B_R = 20000; //后 右
    // shoot_speed.F_L = 20000; //前 左
    // shoot_speed.F_R = 20000; //前 右
    Motor_Init_Config_s friction_f_conf = {
        .motor_type = M3508,
        .can_init_config = {
            .can_handle = &hcan1,
        },
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 5,
                .Kd = 0,
                .Ki = 0.1,
                .DeadBand = 0,
                .Improve = PID_Integral_Limit | PID_OutputFilter |PID_DerivativeFilter,
                .MaxOut = 15000,
                .Output_LPF_RC = 0,
                .Derivative_LPF_RC = 0,
                .IntegralLimit = 10000,
            },
            .current_PID = {
                .Kp = 0,
                .Kd = 0,
                .Ki = 0,
                .DeadBand = 0,
                .Improve = PID_Integral_Limit | PID_OutputFilter | PID_Derivative_On_Measurement,
                .MaxOut = 15000,
                .IntegralLimit = 10000,
                
            },
            //.current_feedforward_ptr = &shoot_ff,
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
            .feedforward_flag = CURRENT_FEEDFORWARD,
        },
    };
    friction_f_conf.can_init_config.tx_id = 1;
    Friction_F_L = DJIMotorInit(&friction_f_conf);
    friction_f_conf.can_init_config.tx_id = 2;
    friction_f_conf.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    Friction_F_R = DJIMotorInit(&friction_f_conf);

    Motor_Init_Config_s friction_b_conf = {
        .motor_type = M3508,
        .can_init_config = {
            .can_handle = &hcan1,
        },
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 5,
                .Kd = 0,
                .Ki = 0.1,
                .DeadBand = 0,
                .Improve = PID_Integral_Limit | PID_OutputFilter | PID_Derivative_On_Measurement,
                .MaxOut = 15000,
                .IntegralLimit = 10000,
                .Output_LPF_RC = 0,
                .Derivative_LPF_RC=0,
            },
            .current_PID = {
                .Kp = 0,
                .Kd = 0,
                .Ki = 0,
                .DeadBand = 0,
                .Improve = PID_Integral_Limit | PID_OutputFilter | PID_Derivative_On_Measurement,
                .MaxOut = 15000,
                .IntegralLimit = 10000,
            },
            //.current_feedforward_ptr = &shoot_ff,
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
            .feedforward_flag = CURRENT_FEEDFORWARD,
        },
    };
    friction_b_conf.can_init_config.tx_id = 3;
    Friction_B_L = DJIMotorInit(&friction_b_conf);
    friction_b_conf.can_init_config.tx_id = 4;
    friction_b_conf.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    Friction_B_R = DJIMotorInit(&friction_b_conf);

    Motor_Init_Config_s load_conf = {
        .motor_type = M2006,
        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 5,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 15,
                .Ki = 2,
                .Kd = 0,
                .DeadBand = 0,
                .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_OutputFilter | PID_Trapezoid_Intergral,
                .Derivative_LPF_RC = 0,
                .Output_LPF_RC = 0,
                .MaxOut = 15000,    
                .IntegralLimit = 500,
            },
            .speed_PID = {
                .Kp = 2,
                .Ki = 0,
                .Kd = 0,
                .DeadBand = 0,
                .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_OutputFilter | PID_Trapezoid_Intergral,
                .Derivative_LPF_RC = 0,
                .Output_LPF_RC = 0,
                .MaxOut = 15000,
                .IntegralLimit = 10000,
            },
            .current_PID = {
                .Kp = 0,
                .Ki = 0,
                .Kd = 0,
                .DeadBand = 0,
                .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_OutputFilter | PID_Trapezoid_Intergral,
                .Derivative_LPF_RC = 0,
                .Output_LPF_RC = 0,
                .MaxOut = 15000,
                .IntegralLimit = 10000,
            },
        },
        .controller_setting_init_config = {
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
        },
    };
    Loader = DJIMotorInit(&load_conf);

    shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

    // USART_Init_Config_s talk = {
    //     .recv_buff_size = 32,
    //     .usart_handle = &huart1,
    //     .module_callback = oled_callback,
    // };

    // blt_usart_instance = USARTRegister(&talk);

    // OLED_Init();
    // OLED_SetColorMode(OLED_COLOR_NORMAL);
    // OLED_SetOrientation(OLED_Orientation_0);
    // OLED_ClearBuffer();
    // OLED_Refresh();

}
/* 机器人发射机构控制核心任务 */
void ShootTask()
{
    // 开启红点激光器
    HAL_GPIO_WritePin(Laser_GPIO_Port, Laser_Pin, GPIO_PIN_SET);
    // USARTSend(blt_usart_instance, shoot_speed_blt_send, sizeof(shoot_speed_blt_send), USART_TRANSFER_DMA);
    // HAL_UART_Receive_DMA(&huart1,shoot_speed_blt_receive,sizeof(shoot_speed_blt_receive));
    // 从cmd获取控制数据
    SubGetMessage(shoot_sub, &shoot_cmd_recv);

    // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
    if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
    {
        DJIMotorStop(Friction_B_L);
        DJIMotorStop(Friction_B_R);
        DJIMotorStop(Friction_F_L);
        DJIMotorStop(Friction_F_R);
        DJIMotorStop(Loader);
    }
    else // 恢复运行
    {
        DJIMotorEnable(Friction_B_L);
        DJIMotorEnable(Friction_B_R);
        DJIMotorEnable(Friction_F_L);
        DJIMotorEnable(Friction_F_R);
        DJIMotorEnable(Loader);
    }
    if (shoot_cmd_recv.friction_mode == FRICTION_ON)
    {
        switch (shoot_cmd_recv.bullet_speed)
        {
        case Test_Speed:
            DJIMotorSetRef(Friction_B_L, 25000);//shoot_speed_blt_send);
            DJIMotorSetRef(Friction_B_R, 25000);//shoot_speed_blt_send);
            DJIMotorSetRef(Friction_F_L, 25000);//shoot_speed_blt_send);
            DJIMotorSetRef(Friction_F_R, 25000);//shoot_speed_blt_send);
            break;
        case Home_Speed:
            DJIMotorSetRef(Friction_B_L, 0);
            DJIMotorSetRef(Friction_B_R, 0);
            DJIMotorSetRef(Friction_F_L, 0);
            DJIMotorSetRef(Friction_F_R, 0);
            break;
        case Tower_Speed:
            DJIMotorSetRef(Friction_B_L, 0);
            DJIMotorSetRef(Friction_B_R, 0);
            DJIMotorSetRef(Friction_F_L, 0);
            DJIMotorSetRef(Friction_F_R, 0);
            break;
        default:
            
            break;
        }
    }
    else // 关闭摩擦轮
    {
        DJIMotorSetRef(Friction_B_L, 0);
        DJIMotorSetRef(Friction_B_R, 0);
        DJIMotorSetRef(Friction_F_L, 0);
        DJIMotorSetRef(Friction_F_R, 0);
    }
    // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
    // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
    // if (hibernate_time + dead_time > DWT_GetTimeline_ms())
    //     return;

    switch (shoot_cmd_recv.load_mode)
    {
    case LOAD_STOP:
        // DJIMotorOuterLoop(Loader, SPEED_LOOP); // 切换到速度环
        // DJIMotorSetRef(Loader, 0);
        break;
    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
    case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
        load_angle_set = Loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE * REDUCTION_RATIO_LOADER;
        DJIMotorOuterLoop(Loader, ANGLE_LOOP);  // 切换到速度环
        DJIMotorSetRef(Loader, load_angle_set); // 增加3发
        hibernate_time = DWT_GetTimeline_ms();  // 记录触发指令的时间
        dead_time = 10000;
        break;
    // 三连发,如果不需要后续可能删除
    case LOAD_2_BULLET: // 完成3发弹丸发射的时间
        break;
    // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
    case LOAD_BURSTFIRE:
        DJIMotorOuterLoop(Loader, SPEED_LOOP);  // 切换到速度环
        DJIMotorSetRef(Loader, shoot_cmd_recv.shoot_rate); // 增加3发
        break;
    default:
        while (1)
            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
    }

    PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
}

//一键发射的摩擦轮启动函数
void shoot_mini(int speed){

    DJIMotorSetRef(Friction_B_L, speed);
    DJIMotorSetRef(Friction_B_R, speed);
    DJIMotorSetRef(Friction_F_L, speed);
    DJIMotorSetRef(Friction_F_R, speed);   
    }

//切换为速度环减少误差累积
void shoot_easy_mode(){
    DJIMotorOuterLoop(Loader, SPEED_LOOP);
    DJIMotorSetRef(Loader, shoot_cmd_recv.shoot_rate); // 增加3发
}

//一键发射，拨盘转动90°
void angle(){
    
    load_angle_set = Loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE * REDUCTION_RATIO_LOADER;
    DJIMotorOuterLoop(Loader, ANGLE_LOOP);  // 切换到速度环
    DJIMotorSetRef(Loader, load_angle_set); // 增加3发
    hibernate_time = DWT_GetTimeline_ms();  // 记录触发指令的时间
    dead_time = 10000;
}

//串口发送函数
// void uart_send(){
//     USARTSend(blt_usart_instance, shoot_speed_blt_send, sizeof(shoot_speed_blt_send), USART_TRANSFER_DMA);
// }

//串口接收函数
// void uart_receive(){
//     HAL_UART_Receive_DMA(&huart1,shoot_speed_blt_receive,sizeof(shoot_speed_blt_receive));
// }

//HAL库的串口接收回调函数
// void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart){
//     if(huart->Instance == USART1){
//         shoot_speed_blt_send =shoot_speed_blt_receive;
//         HAL_UART_Receive_DMA(&huart1,shoot_speed_blt_receive,sizeof(shoot_speed_blt_receive));
//          __HAL_DMA_DISABLE_IT(huart1.hdmarx, DMA_IT_HT);
//     }
// }   

//oled任务
// void oled_task(){
    
//     sprintf(oled_buf, "%d\r\n", shoot_speed);
//     for(;;) {
//     int num = 0;
//       OLED_ClearBuffer();
//       OLED_PrintChinese(0, 20,oled_buf, 16, OLED_COLOR_NORMAL);
//       OLED_Refresh();
//       if(num++ > 2){
//         break;
//       }
//     }
// }